#define BLINKER_PRINT Serial
#define BLINKER_WIFI
#include <Blinker.h>
#include <MPU6050_tockn.h>
#include <Ticker.h>
#include <analogWrite.h>
#include <Arduino.h>
#include <Wire.h>
#define IN1 25
#define IN3 14
#define IN2 33
#define IN4 27
#define PWMA 32
#define PWMB 12
#define STBY 26
#define ENCODER_L 19  //编码器采集引脚 每路2个 共4个
#define DIRECTION_L 18
#define ENCODER_R 17
#define DIRECTION_R 5
#define ZHONGZHI 0//小车的机械中值
#define DIFFERENCE 2
typedef unsigned short U16;
//blinker
char auth[] = "f3e8430daf19";
char ssid[] = "Smartisan personal hotspot";
char pswd[] = "1234567891";
BlinkerButton Button1("qian");
BlinkerButton Button2("hou");
BlinkerButton Button3("left");
BlinkerButton Button4("right");
BlinkerButton Button5("stop");

unsigned char Flag_Stop = 1;//停止标志位
//外部中断
const byte interruptPin1 = 19;
const byte interruptPin2 = 17;
portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
int Angle;//角度显示
volatile long Velocity_L, Velocity_R = 0;   //左右轮编码器数据
int Velocity_Left, Velocity_Right = 0;  //左右轮速度
int Flag_Qian, Flag_Hou, Flag_Left, Flag_Right=0; //遥控相关变量
int Balance_Pwm,Velocity_Pwm,Turn_Pwm;
int Motor1, Motor2;
//PD参数与PI参数
float Balance_Kp=30,Balance_Kd=0.4;
float Velocity_Kp=-3.5,Velocity_Ki=Velocity_Kp/200;


MPU6050 mpu6050(Wire);

//定时中断
Ticker   ticker1;
////检测小车是否被拿起
//int Pick_Up(float Acceleration, float Angle, int encoder_left, int encoder_right){
//  static unsigned int flag, count0, count1, count2 ;
//  if (flag == 0) //第一步
//  {
//    if (abs(encoder_left) + abs(encoder_right) < 15)         count0++;  //条件1，小车接近静止
//    else       count0 = 0;
//    if (count0 > 10)      flag = 1, count0 = 0;
//  }
//  if (flag == 1) //进入第二步
//  {
//    if (++count1 > 400)       count1 = 0, flag = 0;                         //超时不再等待2000ms
//    if (Acceleration > 1 && (Angle > (-14 + ZHONGZHI)) && (Angle < (14 + ZHONGZHI)))  flag = 2; //条件2，小车是在0度附近被拿起
//  }
//  if (flag == 2)  //第三步
//  {
//    if (++count2 > 200)       count2 = 0, flag = 0;       //超时不再等待1000ms
//    if (abs(encoder_left + encoder_right) > 200)           //条件3，小车的轮胎因为正反馈达到最大的转速      
//     {
//        flag = 0;  return 1;
//      }                                           
//  }
//  return 0;
//}
////检测小车是否被放下
//int Put_Down(float Angle, int encoder_left, int encoder_right){
//  static U16 flag, count ;
//  if (Flag_Stop == 0)         return 0;                   //防止误检
//  if (flag == 0)
//  {
//    if (Angle > (-10 + ZHONGZHI) && Angle < (10 + ZHONGZHI) && encoder_left == 0 && encoder_right == 0)      flag = 1; //条件1，小车是在0度附近的
//  }
//  if (flag == 1)
//  {
//    if (++count > 100)       count = 0, flag = 0;  //超时不再等待 500ms
//    if (encoder_left > 12 && encoder_right > 12 && encoder_left < 80 && encoder_right < 80) //条件2，小车的轮胎在未上电的时候被人为转动
//    {
//      flag = 0;
//      flag = 0;
//      return 1;    //检测到小车被放下
//    }
//  }
//  return 0;
//}
////异常关闭电机
//unsigned char Turn_Off(float angle)
//{
//  unsigned char temp;
//  if (angle < -40 || angle > 40 || Flag_Stop == 1 )//===倾角大于40度关闭电机//===Flag_Stop置1关闭电机
//  {                                                                         
//    temp = 1;                                          
//    analogWrite(PWMA, 0);  //PWM输出为0
//    analogWrite(PWMB, 0); //PWM输出为0;
//  }
//  else    temp = 0;   //不存在异常，返回0
//  return temp;
//}
// 按下按键即会执行该函数
void button1_callback(const String & state) {
      if (state=="press") {key_parse(1);} 
      else if(state=="pressup"){key_parse(5);}
}
void button2_callback(const String & state) {
      if (state=="press") {key_parse(2);} 
      else if(state=="pressup"){key_parse(5);}
}
void button3_callback(const String & state) {
       if (state=="press") {key_parse(3);} 
      else if(state=="pressup"){key_parse(5);}
}
void button4_callback(const String & state) {
     if (state=="press") {key_parse(4);} 
      else if(state=="pressup"){key_parse(5);}
}
void button5_callback(const String & state) {
//    BLINKER_LOG("get button state: ", state);
      if (state=="on") {key_parse(5);} 
}
void key_parse(uint8_t car_mode)
{
                switch (car_mode)   {
                  case 1: Flag_Qian = 1, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 0;   break;              //前              
                  case 2: Flag_Qian = 0, Flag_Hou = 1, Flag_Left = 0, Flag_Right = 0;   break;              //后
                  case 3: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 1, Flag_Right = 0;   break;              //左
                  case 4: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 1;   break;               //右
                  case 5: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 0;   break;
                  default: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 0;    break;
                }

}
//转向控制
int turn(float gyro)//转向控制
{
  static float Turn_Target, Turn, Turn_Convert = 3;
  float Turn_Amplitude = 80,  9 = 2, Kd = -0.5;//2,0.5;  //PD参数
  if(1 == Flag_Left  )           Turn_Target += Turn_Convert;  //根据遥控指令改变转向偏差
  else if (1 == Flag_Right)       Turn_Target -= Turn_Convert;//根据遥控指令改变转向偏差
  else Turn_Target = 0;
  if (Turn_Target > Turn_Amplitude)  Turn_Target = Turn_Amplitude; //===转向速度限幅
  if (Turn_Target < -Turn_Amplitude) Turn_Target = -Turn_Amplitude;
  Turn = -Turn_Target * Kp + gyro * Kd;         //===结合Z轴陀螺仪进行PD控制
  return Turn;
}
//读取编码器数据
void READ_ENCODER_L() {
  if (digitalRead(ENCODER_L) == LOW) {     //如果是下降沿触发的中断
    if (digitalRead(DIRECTION_L) == LOW)      Velocity_L--;  //根据另外一相电平判定方向
    else      Velocity_L++;
  }
  else {     //如果是上升沿触发的中断
    if (digitalRead(DIRECTION_L) == LOW)      Velocity_L++; //根据另外一相电平判定方向
    else     Velocity_L--;
  }

}
void READ_ENCODER_R() {
  if (digitalRead(ENCODER_R) == LOW) { //如果是下降沿触发的中断
    if (digitalRead(DIRECTION_R) == LOW)      Velocity_R--;//根据另外一相电平判定方向
    else      Velocity_R++;
  }
  else {   //如果是上升沿触发的中断
    if (digitalRead(DIRECTION_R) == LOW)      Velocity_R++; //根据另外一相电平判定方向
    else     Velocity_R--;
  }
}
//PI速度环函数
int velocity(int encoder_left, int encoder_right){
  static float Velocity, Encoder_Least, Encoder, Movement;
  static float Encoder_Integral, Target_Velocity;
  if       ( 1 == Flag_Qian)Movement = 1600;         //遥控
  else   if ( 1 == Flag_Hou)Movement = -1600;
  else    //这里是停止的时候反转，让小车尽快停下来
  {
    Movement = 0;
    if (Encoder_Integral > 300)   Encoder_Integral -= 200;
    if (Encoder_Integral < -300)  Encoder_Integral += 200;    //遥控
  Encoder_Least = (encoder_left + encoder_right) - 0;               //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
  Encoder *= 0.7;                                                   //===一阶低通滤波器
  Encoder += Encoder_Least * 0.3;                                   //===一阶低通滤波器
  Encoder_Integral += Encoder;                                      //===积分出位移 积分时间：40ms
  Encoder_Integral = Encoder_Integral - Movement;                 //===接收遥控器数据，控制前进后退
  if (Encoder_Integral > 21000)    Encoder_Integral = 21000;        //===积分限幅
  if (Encoder_Integral < -21000) Encoder_Integral = -21000;         //===积分限幅
  Velocity = Encoder * Velocity_Kp + Encoder_Integral * Velocity_Ki;                  //===速度控制
  //if (Turn_Off(mpu6050.getAngleY()) == 1 || Flag_Stop == 1)    Encoder_Integral = 0;//小车停止的时候积分清零
  return Velocity; 
  }
  }

//中断函数
void control() {
    static int Velocity_Count,Turn_Count;
    sei();
    mpu6050.update();
    Angle = mpu6050.getAngleX();
    Balance_Pwm = balance(mpu6050.getAngleX(), mpu6050.getGyroX());
    if (++Velocity_Count >= 8) //速度控制，控制周期40ms
    {
    Velocity_Left = Velocity_L;    Velocity_L = 0;  //读取左轮编码器数据，并清零，这就是通过M法测速（单位时间内的脉冲数）得到速度。
    Velocity_Right = Velocity_R;    Velocity_R = 0; //读取右轮编码器数据，并清零
    Velocity_Pwm = velocity(Velocity_Left, Velocity_Right);//速度PI控制，控制周期40ms
    Velocity_Count = 0;
    }
    if (++Turn_Count >= 4)//转向控制，控制周期20ms
    {
    Turn_Pwm = turn(mpu6050.getGyroZ());
    Turn_Count = 0;
    }
    Motor1 = Balance_Pwm-Velocity_Pwm+ Turn_Pwm;
    Motor2 = Balance_Pwm-Velocity_Pwm- Turn_Pwm;
    Xianfu_Pwm();
    //if (Pick_Up(mpu6050.getAccZ(),mpu6050.getAngleY() , Velocity_Left, Velocity_Right))   Flag_Stop = 1;  //===如果被拿起就关闭电机//===检查是否小车被那起
    //if (Put_Down(mpu6050.getAngleY(), Velocity_Left, Velocity_Right))      Flag_Stop = 0;              //===检查是否小车被放下
    //if (Turn_Off(mpu6050.getAngleY()) == 0)        
    Set_Pwm(Motor1, Motor2);//如果不存在异常，赋值给PWM寄存器控制电机  
}
//设置PWM函数
void Set_Pwm(int moto1,int moto2)
{
  if (moto1 > 0)     digitalWrite(IN1, LOW),       digitalWrite(IN2, HIGH);  //TB6612的电平控制
  else              digitalWrite(IN1, HIGH),      digitalWrite(IN2, LOW);//TB6612的电平控制
  analogWrite(PWMA, abs(moto1)); //赋值给PWM寄存器
  if (moto2 < 0) digitalWrite(IN3, HIGH),     digitalWrite(IN4, LOW); //TB6612的电平控制
  else        digitalWrite(IN3, LOW),      digitalWrite(IN4, HIGH); //TB6612的电平控制
  analogWrite(PWMB, abs(moto2));//赋值给PWM寄存器
}
//PD函数
int balance(float Angle, float Gyro)
{
  float Bias;
  int balance;
  Bias = Angle - 0;   //===求出平衡的角度中值 和机械相关
  balance = Balance_Kp * Bias + Gyro * Balance_Kd; //===计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数
  return balance;
}
//限制PWM
void Xianfu_Pwm(void)
{
  int Amplitude = 250;  //===PWM满幅是255 限制在250
  if(Flag_Qian==1)  Motor2-=DIFFERENCE;  //DIFFERENCE是一个衡量平衡小车电机和机械安装差异的一个变量。直接作用于输出，让小车具有更好的一致性。
  if(Flag_Hou==1)   Motor2-=DIFFERENCE-2;
  if (Motor1 < -Amplitude) Motor1 = -Amplitude;
  if (Motor1 > Amplitude)  Motor1 = Amplitude;
  if (Motor2 < -Amplitude) Motor2 = -Amplitude;
  if (Motor2 > Amplitude)  Motor2 = Amplitude;
}
void setup() {
  //电机引脚设定
  pinMode(IN1, OUTPUT);        //TB6612控制引脚，控制电机1的方向，01为正转，10为反转
  pinMode(IN2, OUTPUT);          //TB6612控制引脚，
  pinMode(IN3, OUTPUT);          //TB6612控制引脚，控制电机2的方向，01为正转，10为反转
  pinMode(IN4, OUTPUT);          //TB6612控制引脚，
  pinMode(PWMA, OUTPUT);         //TB6612控制引脚，电机PWM
  pinMode(PWMB, OUTPUT);         //TB6612控制引脚，电机PWM
  pinMode(STBY, OUTPUT);
  digitalWrite(IN1, 0);          //TB6612控制引脚拉低
  digitalWrite(IN2, 0);          //TB6612控制引脚拉低
  digitalWrite(IN3, 0);          //TB6612控制引脚拉低
  digitalWrite(IN4, 0);          //TB6612控制引脚拉低
  analogWrite(PWMA, 0);          //TB6612控制引脚拉低
  analogWrite(PWMB, 0);          //TB6612控制引脚拉低
  digitalWrite(STBY, 1);
  pinMode(19, INPUT_PULLUP);       //编码器引脚A
  pinMode(18, INPUT);       //编码器引脚B
  pinMode(17, INPUT_PULLUP);       //编码器引脚A
  pinMode(5, INPUT);       //编码器引脚B
  // put your setup code here, to run once:
  Serial.begin(115200);
  delay(3000);
  //MPU
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);
  delay(20); 
// 初始化blinker
  BLINKER_DEBUG.stream(Serial);
  Blinker.begin(auth, ssid, pswd);
  Button1.attach(button1_callback);
  Button2.attach(button2_callback);
  Button3.attach(button3_callback);
  Button4.attach(button4_callback);
  Button5.attach(button5_callback);
  Flag_Qian=0; Flag_Hou=0; Flag_Left=0; Flag_Right=0;
  Blinker.delay(20);
  //中断
  ticker1.attach(0.005, control);
  attachInterrupt(digitalPinToInterrupt(interruptPin1), READ_ENCODER_L , CHANGE);
  attachInterrupt(digitalPinToInterrupt(interruptPin2), READ_ENCODER_R , CHANGE);
}

void loop() {
      Blinker.run();
}